#include "bsp_debug.h"

void CharSend(uint8_t ch)
{
    // if (HAL_UART_GetState(&huart1) != HAL_UART_STATE_BUSY_TX && (__HAL_UART_GET_FLAG(&huart1, UART_FLAG_TC) == SET))
    // HAL_UART_Transmit_DMA(&huart1, &ch, 1);
    HAL_UART_Transmit(&huart1, &ch, 1, 1000);
}

void myitoa(int num, char str[])
{
    int i, j, remain;
    char tmp;
    i = 0;

    do
    {
        remain = num % 10;
        if (remain > 9)
            str[i] = remain - 10 + 'A';
        else
            str[i] = remain + '0';
        i++;
    } while (num /= 10);
    str[i] = '\0';

    for (j = 0; j <= (i - 1) / 2; j++)
    {
        tmp = str[j];
        str[j] = str[i - 1 - j];
        str[i - 1 - j] = tmp;
    }
}
void StrSend(char *str)
{
    // if (HAL_UART_GetState(&huart1) != HAL_UART_STATE_BUSY_TX && (__HAL_UART_GET_FLAG(&huart1, UART_FLAG_TC) == SET))
    // HAL_UART_Transmit_DMA(&huart1, (uint8_t *)str, 30);
    while (*str)
    {
        CharSend((uint8_t)(*str));
        // HAL_UART_Transmit(&huart, (uint8_t *)str, 1, 1000);
        str++;
    }
}

void SerialDebug(const char *format, ...)
{
    char c, ch, str[10];
    va_list ap;
    uint8_t tmp[] = ".0-";

    va_start(ap, format);
    while ((c = *format))
    {
        switch (c)
        {
        case '%':
            ch = *++format;
            switch (ch)
            {
            case 'd':
            {
                int n = va_arg(ap, int);
                myitoa(n, str);
                StrSend(str);
                break;
            }
            case 'f':
            {
                double temp = va_arg(ap, double);
                int n;
                int zero_counter = 0;

                n = temp;
                if (temp < 0)
                {
                    // HAL_UART_Transmit(&huart1, (uint8_t *)&tmp[2], 1, 1000);
                    CharSend((uint8_t)tmp[2]);
                    n = -n;
                    temp = -temp;
                }
                myitoa(n, str);
                StrSend(str);
                // HAL_UART_Transmit(&huart1, (uint8_t *)&tmp[0], 1, 1000);
                CharSend((uint8_t)tmp[0]);
                n = (temp - n) * 10000000;
                temp = n;
                while ((int)temp != 0)
                {
                    zero_counter++;
                    temp /= 10;
                }
                for (temp = 0; temp < 7 - zero_counter; temp++)
                    // HAL_UART_Transmit(&huart1, (uint8_t *)&tmp[1], 1, 1000);
                    CharSend((uint8_t)tmp[1]);
                if (n > 0)
                {
                    myitoa(n, str);
                    StrSend(str);
                }
                else if (n < 0)
                {
                    myitoa(-n, str);
                    StrSend(str);
                }
                break;
            }
            default:
            {
                break;
            }
            }
            break;
        default:
            // HAL_UART_Transmit(&huart1, (uint8_t *)&c, 1, 1000);
            CharSend((uint8_t)c);
            break;
        }
        format++;
    }
    va_end(ap);
}
